#include "control.h"
#include <vector>
#include <iostream>

using namespace std;

RobotController::RobotController()
{
	mode = Forward;
	yaw_pid.set_target(0);
	yaw_pid.set_Limitval(1);
	yaw_pid.set_param(0, 0, 0);

	side_pid.set_target(320);
	side_pid.set_Limitval(0.5);
	side_pid.set_param(0, 0, 0);

	leftLine._color = Scalar(255, 0, 0);
	rightLine._color = Scalar(0, 255, 0);
	midLine._color = Scalar(0, 0, 255);
}

/**
 * @brief 利用种子法确定3边的所有离散点
 *				利用离散点进行最小二乘法直线拟合,并改变机器人的状态
 * @param pred 预测结果(单通道)
*/
void RobotController::get_Line(Mat& pred, Mat& H)
{
	/* initialize */
	const uint16_t row = pred.rows;
	const uint16_t col = pred.cols;
	const uint16_t start_row = row - 10;
	const uint16_t end_row = (row / 2) + 20;
	const uint8_t step = 20;
	const uint8_t max_samplePts = 1 + (row / 2 - 30) / step;
	const uint8_t bord_thresh = 3;
	const uint8_t position_thresh = 40;
	uint16_t x0 = col / 2;
	uint8_t Pts_Left = 0;		//x=0的点的个数
	uint8_t Pts_Right = 0;	//x=640的点的个数

	if (leftLine.pts != nullptr)
		delete leftLine.pts;
	if (rightLine.pts != nullptr)
		delete rightLine.pts;
	if (midLine.pts != nullptr)
		delete midLine.pts;

	leftLine.pts = new vector<Point>;
	rightLine.pts = new vector<Point>;
	midLine.pts = new vector<Point>;

	/* 1.确定所有离散点 */
	for (uint16_t idx = start_row; idx > end_row; idx -= step)
	{
		uint8_t* row_ptr = pred.ptr<uint8_t>(idx); //指向当前行且x = 0的指针
		int x1 = 0, x2 = 0;

		// (1)种子初始点在盲道上
		if (row_ptr[x0] != 0)
		{
			/* 从中线向左边遍历，确定该行的左边界 */
			for (x1 = x0; x1 > 0; --x1)
			{
				if (row_ptr[x1] == 0)
					break;
			}
			if (x1 == 0)
				Pts_Left++;
			leftLine.pts->push_back(Point(x1, idx));

			/* 从中线向右边遍历，确定该行的右边界 */
			for (x2 = x0; x2 < col; ++x2)
			{
				if (row_ptr[x2] == 0)
					break;
			}
			if (x2 == col)
				Pts_Right++;
			rightLine.pts->push_back(Point(x2, idx));
		}

		// (2)种子初始点不在盲道上
		else if (row_ptr[x0] == 0)
		{
			// (2.1)最左边的点是背景点,从左往右扫描,先找左边界 
			if (row_ptr[0] == 0) 
			{
				/* 左边界确定 */
				for (x1 = 0; x1 < col; ++x1)
				{
					if (row_ptr[x1] == 1)
						break;
				}
				if (x1 == col) //说明本行没有盲道元素,直接跳过
				{
					continue;
				}
				leftLine.pts->push_back(Point(x1, idx));

				/* 右边界确定 */
				for (x2 = x1; x2 < col; ++x2)
				{
					if (row_ptr[x2] == 0)
						break;
				}
				if (x2 == col)
					Pts_Right++;
				rightLine.pts->push_back(Point(x2, idx));
			}

			// (2.2)最右边的点是背景点,从右往左扫描,先找右边界
			else if (row_ptr[col-1] == 0)
			{
				/* 右边界确定 */
				for (x2 = col - 1; x2 > 0; --x2)
				{
					if (row_ptr[x2] == 1)
						break;
				}
				if (x2 == 0) //说明本行没有盲道元素,直接跳过
				{
					continue;
				}
				rightLine.pts->push_back(Point(x2, idx));

				/* 左边界确定 */
				for (x1 = x2; x1 > 0; --x1)
				{
					if (row_ptr[x1] == 0)
						break;
				}
				if (x1 == 0)
					Pts_Left++;
				leftLine.pts->push_back(Point(x1, idx));
			}
		}

		/* 中点确定 */
		x0 = (x1 + x2) / 2;
		midLine.pts->push_back(Point(x0, idx));
	}

	/* 2.确定左右边线的状态 */
	uint8_t num_sample = midLine.pts->size(); //实际采集到的离散点个数
	if (num_sample == 0)
	{
		mode = LoseLine;
	}

	else if (num_sample < max_samplePts - bord_thresh)
	{
		mode = Obstacle;
	}

	else
	{
		(Pts_Left >= bord_thresh) ? (leftLine.sta = Line::Border) : (leftLine.sta = Line::Normal);
		(Pts_Right >= bord_thresh) ? (rightLine.sta = Line::Border) : (rightLine.sta = Line::Normal);
		midLine.fitting_BEV(H); //先把中线的离散点转到BEV再计算
		/* 判断四足相对于盲道的位置 */
		if (midLine.mid.x > 320 + position_thresh)
		{
			position = Left;
		}
		else if (midLine.mid.x < 320 - position_thresh)
		{
			position = Right;
		}
		else
		{
			position = Mid;
		}

		/* 判断运动模式 */
		if (leftLine.sta == Line::Normal && rightLine.sta == Line::Normal)
		{
			mode = Forward;
		}
		else
		{
			mode = Horizontal;
		}
	}

}

/**
 * @brief 由当前模式及相关信息计算速度
*/
void RobotController::calculate_speed(void)
{
	if (mode == Forward)
	{
		/* 前进速度 */
		forward_speed = 0.6f;

		/* 水平速度 */
		side_pid.set_param(0.003, 0, 0);
		side_pid.calculate_Error(midLine.mid.x);
		side_speed = side_pid.get_Output();

		/* 旋转速度 */
		yaw_pid.set_param(0.02, 0, 0);
		yaw_pid.calculate_Error(midLine.angle);
		yaw_speed = yaw_pid.get_Output();
	}

	else if (mode == Horizontal)
	{
		/* 前进速度 */
		forward_speed = 0.3f;

		/* 水平速度 */
		side_pid.set_param(0.003, 0, 0.003);
		side_pid.calculate_Error(midLine.mid.x);
		side_speed = side_pid.get_Output();

		/* 旋转速度 */
		yaw_pid.set_param(0.01, 0, 0);
		yaw_pid.calculate_Error(midLine.angle);
		yaw_speed = yaw_pid.get_Output();
	}

	else if (mode == Obstacle) //该模式只直行
	{
		/* 前进速度 */
		forward_speed = 0.4f;

		/* 水平速度 */
		side_speed = 0;

		/* 旋转速度 */
		yaw_speed = 0;
	}

	else if (mode == LoseLine) //自转直到找到线
	{
		/* 前进速度 */
		forward_speed = 0;

		/* 狗在盲道右边,向左寻找盲道 */
		if (position == Left)
		{
			yaw_speed = 1;
			side_speed = 0.1;
		}

		/* 狗在盲道左边,向右寻找盲道 */
		else if (position == Right)
		{
			yaw_speed = -1;
			side_speed = -0.1;
		}

		else 
		{
			yaw_speed = 0;
			side_speed = 0;
		}

	}
}